import { _decorator, Component, Node, Vec3, Vec4, Vec2, clamp, clamp01, EPSILON, Mesh } from 'cc';
import { Spline } from './spline';
const { ccclass, property } = _decorator;

let __v0 = new Vec3();
let __v1 = new Vec3();
let __dis = new Vec3();
let __indices = new Vec2();
let v3_1 = new Vec3();
let v3_2 = new Vec3();

function vec3ClosestPointOnSegment (p: Vec3, s0: Vec3, s1: Vec3) {
    let s = Vec3.subtract(v3_1, s1, s0);
    let dd = Vec3.lengthSqr(s);
    if (dd < EPSILON) {
        return 0;
    }
    return clamp01(Vec3.dot(Vec3.subtract(v3_2, p, s0), s) / dd); //use sqrt(dd)? 
}


@ccclass('SmoothPath')
export class SmoothPath {
    private _looped: boolean = false;
    private _localControlPoints1: Vec3[] = [];
    private _localControlPoints2: Vec3[] = [];
    private _positions: Vec3[] = [];

    /**
     * 构造SmoothPath
     * @param positions 
     * @param isLoop 
     */
    constructor (positions: Vec3[], isLoop = false) {
        // this._node = node;
        this._positions = positions;
        this._looped = isLoop;

        this._updateControlPoints();
    }

    get looped () {
        return this._looped;
    }
    set looped (v: boolean) {
        if (this._looped !== v) {
            this._looped = v;
        }
    }

    private get _minProgress () {
        return 0;
    }

    private get _maxProgress () {
        let count = this._positions.length - 1;
        if (count < 1) return 0;
        return this.looped ? count + 1 : count;

    }

    /**
     * 更新控制节点
     */
    private _updateControlPoints () {
        let numPoints = this._positions.length;
        if (numPoints > 1) {
            let p1 = new Array<Vec4>(numPoints);
            let p2 = new Array<Vec4>(numPoints);
            let K = new Array<Vec4>(numPoints);
            for (let i = 0; i < numPoints; i++) {
                p1[i] = new Vec4();
                p2[i] = new Vec4();
                let p = this._positions[i];
                K[i] = new Vec4(p.x, p.y, p.z, 0);
            }

            if (this.looped) {
                Spline.computeSmoothControlPointsLooped(K, p1, p2);
            }
            else {
                Spline.computeSmoothControlPoints(K, p1, p2);
            }

            this._localControlPoints1.length = numPoints;
            this._localControlPoints2.length = numPoints;

            for (let i = 0; i < numPoints; i++) {
                this._localControlPoints1[i] = new Vec3(p1[i].x, p1[i].y, p1[i].z);
                this._localControlPoints2[i] = new Vec3(p2[i].x, p2[i].y, p2[i].z);
            }
        }
    }

    private _getBoundingIndices (t: number, out: Vec2) {
        t = this._clampProgress(t);
        let numWaypoints = this._positions.length;
        if (numWaypoints < 2) {
            out.x = out.y = 0;
        }
        else {
            out.x = Math.floor(t);
            if (out.x >= numWaypoints) {
                // Only true if looped
                t -= this._maxProgress;
                out.x = 0;
            }
            out.y = out.x + 1;
            if (out.y == numWaypoints) {
                if (this.looped) {
                    out.y = 0;
                }
                else {
                    --out.y;
                    --out.x;
                }
            }
        }
        return t;
    }

    private _clampProgress (t: number) {
        let maxPos = this._maxProgress;
        if (this._looped && maxPos > 0) {
            t = t % maxPos;
            if (t < 0) {
                t += maxPos;
            }
            return t;
        }
        return clamp(t, 0, maxPos);
    }


    public evaluatePosition (out: Vec3, t: number) {
        //return this.evaluateLocalPosition(out, t).add(this._node.worldPosition);
        return this.evaluateLocalPosition(out, t);
    }

    public evaluateLocalPosition (out: Vec3, t: number) {
        //   let children = this._nodes;
        if (this._positions.length === 0) {
            return out.set(Vec3.ZERO);
        }
        let indices = __indices.set(0, 0);
        t = this._getBoundingIndices(t, indices);
        if (indices.x === indices.y) {
            return out.set(this._positions[indices.x]);
        }
        else {
            return Spline.bezier3(out, t - indices.x, this._positions[indices.x], this._localControlPoints1[indices.x], this._localControlPoints2[indices.x], this._positions[indices.y])
        }
    }

    public findClosestPoint (p: Vec3, startSegment: number, searchRadius: number, stepPerSegment: number) {
        let start = this._minProgress, end = this._maxProgress;
        if (searchRadius >= 0) {
            let r = Math.floor(Math.min(searchRadius, Math.max(1, (end - start) / 2)));
            start = startSegment - r;
            end = startSegment + r + 1;
            if (!this.looped) {
                start = Math.max(start, this._minProgress);
                end = Math.min(end, this._maxProgress);
            }
        }
        let bestPos = startSegment, bestDistance = Infinity;
        let stepSize = 1 / clamp(stepPerSegment, 1, 100);
        let v0 = this.evaluatePosition(__v0, 0);
        for (let f = start + stepSize; f <= end; f += stepSize) {
            let v1 = this.evaluatePosition(__v1, f);
            let t = vec3ClosestPointOnSegment(p, v0, v1);
            let d = Vec3.squaredDistance(p, Vec3.lerp(__dis, v0, v1, t));
            if (d < bestDistance) {
                bestDistance = d;
                bestPos = f - (1 - t) * stepSize;
            }
            v0.set(v1);
        }
        return bestPos;
    }


    /**
     * 获取绘制的贝塞尔曲线的点
     * @param step 步长 越多越平滑
     * @param gird 是否显示成格子
     * @returns 
     */
    public getBezierPositions (step: number = 1 / 10, gird = false) {
        this._updateControlPoints();
        let vertices: Vec3[] = [];

        for (let t = 0; t < this._positions.length; t += step) {
            vertices.push(this.evaluateLocalPosition(new Vec3(), t));
        }
        let positions: Vec3[] = [];
        let point0s = [], point1s = [];

        function linkPoints (p0: Vec3, p1: Vec3) {
            if (!(p0 && p1)) return;
            positions.push(p0);
            positions.push(p1);
        }

        for (let i = 0; i + 1 < vertices.length; i++) {
            let p0 = vertices[i];
            let p1 = vertices[i + 1];
            if (gird) {
                let dir = new Vec3(p1).subtract(p0).normalize();
                let offset = new Vec3(dir.z, dir.y, -dir.x).multiplyScalar(0.1);
                if (i === 0) {
                    point0s.push(new Vec3(p0).subtract(offset));
                    point1s.push(new Vec3(p0).add(offset));
                }
                point0s.push(new Vec3(p1).subtract(offset));
                point1s.push(new Vec3(p1).add(offset));

                linkPoints(new Vec3(p0).subtract(offset), new Vec3(p0).add(offset));
            }
            linkPoints(p0, p1);
        }
        if (this._looped) {
            linkPoints(vertices[vertices.length - 1], vertices[0]);
        }
        for (let i = 0; i + 1 < point0s.length; i++) {
            linkPoints(point0s[i], point0s[i + 1]);
        }
        if (this._looped) {
            linkPoints(point0s[point0s.length - 1], point0s[0]);
        }
        for (let i = 0; i + 1 < point1s.length; i++) {
            linkPoints(point1s[i], point1s[i + 1]);
        }
        if (this._looped) {
            linkPoints(point1s[point1s.length - 1], point1s[0]);
        }

        return positions;
    }
}

